/*
 * @Author: czu_xy 3322566362@qq.com
 * @Date: 2025-05-06 16:28:01
 * @LastEditors: czu_xy 3322566362@qq.com
 * @LastEditTime: 2025-05-24 15:20:24
 * @FilePath: \ProjectSTM32F407c:\Users\33225\Desktop\drone\ti-drone-master\ti-drone-master\ANO_LX_FC_F407\DriversBsp\bluetooth.c
 * @Description: ????????????????,?????????`customMade`, ???koroFileHeader????????? ????????????: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "bluetooth.h"
#include "Drv_AnoOf.h"
#include "Ano_Math.h"
#include "Drv_UWB.h"
#include "string.h"
#include "stdint.h"
#include "stdio.h"


bluetoothStructDef BLTH;

void Bletooth_Uart(void){
    BLTH.x = (int)(UWB_dat.FanlX * 100);
    BLTH.y = (int)(UWB_dat.FanlY * 100);
    send_custom_format(BLTH.x,BLTH.y,BLTH.Data);
    Usart_SendStr_length(BLTH.Data,BLUESIZE);
}


void Usart_SendStr_length(uint8_t *str, uint32_t strlen)
{
    for (uint32_t i = 0; i < strlen; i++) {
        Usart_SendByte(str[i]);
    }
}

void Usart_SendByte(uint8_t byte)
{
    while (!(USART3->SR & USART_SR_TXE));  // ????????
    USART_SendData(USART3, byte);          // ??????
}

void send_custom_format(int16_t x, int16_t y, uint8_t* data) {
    data[0] = 0xA0;    // SOF1
    data[1] = 0xB0;    // SOF2
    data[2] = (uint8_t)(x >> 8);
    data[3] = (uint8_t)(x & 0xFF);
    data[4] = (uint8_t)(y >> 8);
    data[5] = (uint8_t)(y & 0xFF);

    // 计算校验和
    uint16_t sum = 0;
    for (int i = 0; i < 6; i++) {
        sum += data[i];
    }
    data[6] = (uint8_t)(sum & 0xFF);

    data[7] = 0xC0;    // EOF（帧尾）
}
